#include "sbus.h"

#include "main.h"

static int16_t RC_DIFF[4] = {138,5,-172,70};
uint16_t rc_data[16] = {1000, 1000, 1500, 1000};
enum RC_STATUS_ENUM which_rc = RC_RC;//RC_CLIENT;
//enum RC_STATUS_ENUM which_rc = RC_CLIENT;
uint16_t recv_rc_cnt = 0;

uint16_t rc_client_recv_cnt = 0;

void sbus_recv(uint8_t *buf)
{
    if (sbus_parse(buf, rc_data) == 0)
    {
//        for (int i = 0; i < 4; i++)
//        {
//            printf("%d ", rc_data[i]);
//        }
//        printf("\r\n");
    }
}

void rc_loop(void)
{
	if(rc_client_recv_cnt > 0)
	{
		rc_client_recv_cnt = 0;
	}else{
		rc_data[0] = 1000;
		rc_data[1] = 1000;
		rc_data[2] = 1500;
		rc_data[3] = 1000;
	}
}

int sbus_parse(uint8_t *buf, uint16_t *val)
{
	if(which_rc == RC_RC)
	{
		//recv_rc_cnt = 0;
		
		if (buf[0] != 0x0F)
			return 1;

		if (buf[24] != 0x00)
			return 1;

		val[0] = ((buf[1] | buf[2] << 8) & 0x07FF) + RC_DIFF[0];
		val[1] = ((buf[2] >> 3 | buf[3] << 5) & 0x07FF) + RC_DIFF[1];
		val[2] = ((buf[3] >> 6 | buf[4] << 2 | buf[5] << 10) & 0x07FF) + RC_DIFF[2];
		val[3] = ((buf[5] >> 1 | buf[6] << 7) & 0x07FF) + RC_DIFF[3];
		val[4] = ((buf[6] >> 4 | buf[7] << 4) & 0x07FF);
		val[5] = ((buf[7] >> 7 | buf[8] << 1 | buf[9] << 9) & 0x07FF);
		val[6] = ((buf[9] >> 2 | buf[10] << 6) & 0x07FF);
		val[7] = ((buf[10] >> 5 | buf[11] << 3) & 0x07FF);
		val[8] = ((buf[12] | buf[13] << 8) & 0x07FF);
		val[9] = ((buf[13] >> 3 | buf[14] << 5) & 0x07FF);
		val[10] = ((buf[14] >> 6 | buf[15] << 2 | buf[16] << 10) & 0x07FF);
		val[11] = ((buf[16] >> 1 | buf[17] << 7) & 0x07FF);
		val[12] = ((buf[17] >> 4 | buf[18] << 4) & 0x07FF);
		val[13] = ((buf[18] >> 7 | buf[19] << 1 | buf[20] << 9) & 0x07FF);
		val[14] = ((buf[20] >> 2 | buf[21] << 6) & 0x07FF);
		val[15] = ((buf[21] >> 5 | buf[22] << 3) & 0x07FF);
	}
    return 0;
}

enum RECV_FLAG
{
    RECV_FLAG_WAIT = 0,
    RECV_FLAG_HEAD1,
    RECV_FLAG_HEAD2,
    RECV_FLAG_LEN1,
    RECV_FLAG_LEN2,
    RECV_FLAG_CMD,
    RECV_FLAG_DATA,
    RECV_FLAG_SUM
};

#define RECV_MAX_BUF_SIZE 128
struct s_r_frame
{
    enum RECV_FLAG recv_flg;
    uint16_t len;
    uint8_t cmd;
    uint8_t buf[RECV_MAX_BUF_SIZE + 3];
    uint16_t recv_buf_count;
};

struct s_r_frame s_r_frame;

void struct_frame_init(void)
{
    s_r_frame.recv_flg = RECV_FLAG_WAIT;
}
void recv_frame(void)
{
	uint16_t rc_data_tmp[4];
	
	if(which_rc == RC_CLIENT)
	{
        rc_client_recv_cnt++;
		
		rc_data_tmp[0] = (uint16_t)((s_r_frame.buf[3] << 8) | s_r_frame.buf[4]);
		rc_data_tmp[1] = (uint16_t)((s_r_frame.buf[5] << 8) | s_r_frame.buf[6]);
		rc_data_tmp[2] = (uint16_t)((s_r_frame.buf[7] << 8) | s_r_frame.buf[8]);
		rc_data_tmp[3] = (uint16_t)((s_r_frame.buf[9] << 8) | s_r_frame.buf[10]);
		
		LIMIT(rc_data_tmp[0],0,2000);
		LIMIT(rc_data_tmp[1],0,2000);
		LIMIT(rc_data_tmp[2],0,2000);
		LIMIT(rc_data_tmp[3],0,2000);
		
		rc_data[0] = rc_data_tmp[0];
		rc_data[1] = rc_data_tmp[1];
		rc_data[2] = rc_data_tmp[2];
		rc_data[3] = rc_data_tmp[3];
        
	}
	
}

void client_recv(uint8_t *buf, int len)
{
    uint16_t i = 0, i_cnt = 0;
    uint8_t check_sum = 0;

    for (i = 0; i < len; i++)
    {
        switch (s_r_frame.recv_flg)
        {
        case RECV_FLAG_WAIT:
            if (buf[i] == 0xAA)
            {
                s_r_frame.recv_flg = RECV_FLAG_HEAD1;
            }
            break;
        case RECV_FLAG_HEAD1:
            if (buf[i] == 0xBB)
            {
                s_r_frame.len = 0;
                s_r_frame.recv_flg = RECV_FLAG_HEAD2;
                s_r_frame.recv_buf_count = 0;
            }
            else
            {
                if (buf[i] == 0xAA)
                {
                    s_r_frame.recv_flg = RECV_FLAG_HEAD1;
                }
            }
            break;
        case RECV_FLAG_HEAD2:
            s_r_frame.buf[s_r_frame.recv_buf_count++] = buf[i];
            s_r_frame.len = (uint16_t)(buf[i] << 8);
            s_r_frame.recv_flg = RECV_FLAG_LEN1;
            break;
        case RECV_FLAG_LEN1:
            s_r_frame.buf[s_r_frame.recv_buf_count++] = buf[i];
            s_r_frame.len |= buf[i];
            if (s_r_frame.len <= (RECV_MAX_BUF_SIZE + 3))
            {
                s_r_frame.recv_flg = RECV_FLAG_LEN2;
            }
            else
            {
                if (buf[i] == 0xAA)
                {
                    s_r_frame.recv_flg = RECV_FLAG_HEAD1;
                }
            }
            break;
        case RECV_FLAG_LEN2:
            s_r_frame.buf[s_r_frame.recv_buf_count++] = buf[i];

            s_r_frame.cmd = buf[i];
            s_r_frame.recv_flg = RECV_FLAG_CMD;

            break;
        case RECV_FLAG_CMD:

            s_r_frame.buf[s_r_frame.recv_buf_count++] = buf[i];
            if (s_r_frame.recv_buf_count >= s_r_frame.len)
            {

                s_r_frame.recv_flg = RECV_FLAG_DATA;
            }

            break;
        case RECV_FLAG_DATA:
            check_sum = 0;
            for (i_cnt = 0; i_cnt < s_r_frame.len; i_cnt++)
            {
                check_sum += s_r_frame.buf[i_cnt];
            }

            if (check_sum == buf[i])
            {
                recv_frame();
                s_r_frame.recv_flg = RECV_FLAG_WAIT;
				
            }else{
                if (buf[i] == 0xAA)
                {
                    s_r_frame.recv_flg = RECV_FLAG_HEAD1;
                }
			}

            break;

        default:
            if (buf[i] == 0xAA)
            {
                s_r_frame.recv_flg = RECV_FLAG_HEAD1;
            }
            break;
        }
    }
}